package com.dmatek.uwb.packet.bean;

import com.dmatek.uwb.packet.ianalysis.IFilterAlgorithm;

/**
 * 卡尔曼滤波类
 * @author zhangfu
 * @data 2018年9月13日 上午9:09:42
 * @Description
 */
public class KalmanFilter implements IFilterAlgorithm {
	private double X,P,Q,R,Gain;
	public KalmanFilter(double x) {
		super();
		this.X = x; // 系统状态初始化
		this.P = 0.1f; // 协方差初始化，一般不为0，可以选择一个较小的适当的数
		this.Q = 0.2f; // 过程噪声初始化
		this.R = 1; // 测量噪声初始化
	}
	public KalmanFilter(double x,double r) {
		super();
		this.X = x;     //系统状态初始化
        this.P = 0.1f;     //协方差初始化，一般不为0，可以选择一个较小的适当的数
        this.Q = 0.2f;     //过程噪声初始化
        this.R = r;     //测量噪声初始化
	}
	public KalmanFilter(double x, double p, double q, double r) {
		super();
		 this.X = x;     //系统状态初始化
         this.P = p;     //协方差初始化，一般不为0，可以选择一个较小的适当的数
         this.Q = q;     //过程噪声初始化
         this.R = r;     //测量噪声初始化
	}
	@Override
	public double filter(double mesure) throws Exception {
        this.P = this.P + this.Q;                           //预测下一时刻的协方差
        this.Gain = this.P / (this.P + this.R);             //更新卡尔曼增益
        this.X = this.X + this.Gain * (mesure - this.X);    //得到现在状态的最优化估算值
        this.P = (1 - this.Gain) * this.P;                  //更新这一时刻的协方差
        return this.X;
	}
	public double getX() {
		return X;
	}
	public void setX(double x) {
		X = x;
	}
	public double getP() {
		return P;
	}
	public void setP(double p) {
		P = p;
	}
	public double getQ() {
		return Q;
	}
	public void setQ(double q) {
		Q = q;
	}
	public double getR() {
		return R;
	}
	public void setR(double r) {
		R = r;
	}
	public double getGain() {
		return Gain;
	}
	public void setGain(double gain) {
		Gain = gain;
	}
}
